from util_3d import *
from spatialmath.base import *
# 观察到的二维码实际位置

# 上标注T下标
# 1 改变示教抓取位置，并采用rokae api 记录 base_t_end_grab_teach_rokae_type
# 2 改变记录观察位置  base_t_end_obs_teach_rokae_type
# 3 获得观察位置标定板的位姿 cam_teach_t_calib_teach_halcon_type
# 4 如果外参又调整， 调整外参

class Alg:
    def __init__(self):
        # ok
        # 外参
        self.out_param_halcon_type = [-0.128855, 0.0288219, -0.0601685, 89.6343, 0.575319, 181.158, 2]

        # [-0.128812, 0.019791, -0.0605151, 89.4655, 3.72946, 181.324, 2]
                                   #   [-0.128855, 0.0288219, -0.0601685, 89.6343, 0.575319, 181.158, 2]
        self.cam_teach_t_calib_teach_halcon_type = [0.0009234372234002975, 0.0021235158922578183, 0.2470577764991765,
                                                    359.2878838348385, 358.9766207000455, 180.1256866720801, 0]

        self.base_t_end_obs_teach_rokae_type = [1.0500418591034264, 0.4139485479688117, 0.16990508851125896,
                                                -1.5675805865323031, -0.012611012607728019, -1.5609540644381419]

        self.base_t_end_grab_teach_rokae_type = [0.9069589157594569, 0.238780680742038, -0.014386591557805993,
                                                 -1.5676612467638922, -0.012611221636270573, -1.5620016462034911]

        #  手动调整观察时候的末端位姿
        self.base_t_end_obs_teach = rokae_pose_to_T(self.base_t_end_obs_teach_rokae_type)
        #  手动调整观察时候的末端位姿的逆
        self.end_obs_teach_t_base = trinv(self.base_t_end_obs_teach)

        #  外参
        self.end_obs_teach_t_cam_obs_teach = halcon_pose_to_T(self.out_param_halcon_type)
        # 外参的逆
        self.cam_obs_teach_t_end_obs_teach = trinv(self.end_obs_teach_t_cam_obs_teach)

        # 标定板的在相机下面的位姿
        self.cam_teach_t_calib_teach = halcon_pose_to_T(self.cam_teach_t_calib_teach_halcon_type)
        # 标定板的在相机下面的位姿的逆
        self.calib_teach_t_cam_teach = trinv(self.cam_teach_t_calib_teach)

        self.base_t_end_grab_teach = rokae_pose_to_T(self.base_t_end_grab_teach_rokae_type)
        self.end_grab_teach_t_base = trinv(self.base_t_end_grab_teach)

        self.calib_teach_t_end_grab_teach = (self.calib_teach_t_cam_teach @ self.cam_obs_teach_t_end_obs_teach @
                                             self.end_obs_teach_t_base @ self.base_t_end_grab_teach)

        self.calib_real_t_end_grab_real = self.calib_teach_t_end_grab_teach
        # a = T_to_xyzrpy(self.calib_real_t_end_grab_real)
        # a[3] = -91.0
        # a[4] = 1.0
        # self.calib_real_t_end_grab_real = xyzrpy_to_T(a[0], a[1], a[2], a[3], a[4], a[5])

        self.cam_obs_teach_t_calib_real = transl(0, 0, 0)
        self.base_t_end_grab_real = transl(0, 0, 0)

    def gen_base_t_end_grab_real(self, cam_obs_teach_t_calib_real):
        self.cam_obs_teach_t_calib_real = cam_obs_teach_t_calib_real
        self.base_t_end_grab_real = (self.base_t_end_obs_teach @ self.end_obs_teach_t_cam_obs_teach @
                                     self.cam_obs_teach_t_calib_real @ self.calib_real_t_end_grab_real)
        return self.base_t_end_grab_real
    # 1 real  实际运行
    # 2 teach 示教
def calcu_base1_t_base2(end_t_cam, base1_t_end1, cam1_t_calib, base2_t_end2, cam2_t_calib):
    end1_t_cam1 = end_t_cam
    end2_t_cam2 = end_t_cam
    calib_t_cam2 = trinv(cam2_t_calib)
    cam2_t_end2 = trinv(end2_t_cam2)
    end2_t_base2 = trinv(base2_t_end2)
    base1_t_base2 = base1_t_end1@end1_t_cam1@cam1_t_calib@calib_t_cam2@cam2_t_end2@end2_t_base2
    return base1_t_base2


# 1 moveto to obs pose
# 2 get cam_obs_teach_t_calib_real
# 3 gen_base_t_end_grab_real
# 4 movel base_t_end_grab_real with z - 100mm
# 4 movel base_t_end_grab_real
